{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "try:\n",
    "  # We must install required packages if we are in Google Colab\n",
    "  import google.colab\n",
    "  %pip install roboticstoolbox-python>=1.0.2\n",
    "  %pip install sympy\n",
    "except:\n",
    "  # We are not in Google Colab\n",
    "  # Apply custon style to notebook\n",
    "  from IPython.core.display import HTML\n",
    "  import pathlib\n",
    "  styles_path = pathlib.Path(pathlib.Path().absolute(), \"style\", \"style.css\")\n",
    "  styles = open(styles_path, \"r\").read()\n",
    "  HTML(f\"<style>{styles}</style>\")"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 3.0 Analytic Forms\n",
    "\n",
    "$\\large{\\text{Manipulator Differential Kinematics}} \\\\ \\large{\\text{Part II: Acceleration and Advanced Applications}}$\n",
    "\n",
    "$\\text{By Jesse Haviland and Peter Corke}$\n",
    "\n",
    "<br>\n",
    "\n",
    "The sections of the Tutorial paper related to this notebook are listed next to each contents entry.\n",
    "It is beneficial to read these sections of the paper before attempting the notebook Section.\n",
    "\n",
    "We start with some important background theory on the Special Orthogonal Group in Sections 3.1 - 3.4. Skip ahead to Section 3.5 if you want to go straight to the analytical Jacobian.\n",
    "\n",
    "### Contents\n",
    "\n",
    "[3.1 Lie Algebra and the Tangent Space – $\\bf{so}(3)$](#so3t)\n",
    "\n",
    "[3.2 Special Orthogonal Group – $\\bf{SO}(3)$ ](#so31)\n",
    "\n",
    "[3.3 Derivatives of $\\bf{SO}(3)$ and the Angular Jacobian](#so3d)\n",
    "\n",
    "[3.4 Second Derivatives of $\\bf{SO}(3)$ and the Angular Hessian](#acc)\n",
    "\n",
    "[3.5 The Analytic Jacobian](#Ja)\n",
    "* Analytical Form\n",
    "\n",
    "[3.6 Derivative of the Analytic Jacobian](#Jad)\n",
    "* Analytical Form"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "# We will do the imports required for this notebook here\n",
    "\n",
    "# numpy provides import array and linear algebra utilities\n",
    "import numpy as np\n",
    "\n",
    "# the robotics toolbox provides robotics specific functionality\n",
    "import roboticstoolbox as rtb\n",
    "\n",
    "# spatial math provides objects for representing transformations\n",
    "import spatialmath as sm\n",
    "import spatialmath.base as smb\n",
    "\n",
    "# sympy provides symbolic mathematic operations \n",
    "import sympy as sym\n",
    "\n",
    "# the math module\n",
    "import math\n",
    "\n",
    "# skew and vex operations (see Part 1 Notebook 2)\n",
    "from spatialmath.base import skew, vex\n",
    "\n",
    "# The matrix exponential method\n",
    "from scipy.linalg import expm\n",
    "\n",
    "# setting up sympy to print nicely\n",
    "from IPython import display as ds\n",
    "from sympy.physics.mechanics import init_vprinting\n",
    "from sympy.physics.vector import vlatex\n",
    "init_vprinting(use_latex='mathjax')"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='so3t'></a>\n",
    "\n",
    "### 3.1 Lie Algebra and the Tangent Space – $\\bf{so}(3)$\n",
    "---\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p class=\"asasasas\">\n",
    "\n",
    "Rotations in 3-dimensions can be represented by matrices which form Lie groups and which have Lie algebras. These rotations form the Special Orthogonal group in 3 dimensions, denoted as $\\bf{SO}(3)$.\n",
    "\n",
    "</p>"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "An implication of being a Lie group is that there is a smooth and differentiable manifold structure. At any point on the manifold we can construct tangent vectors. The set of all tangent vectors at that point form a vector space - the tangent space. This is the multidimensional equivalent to a tangent line on a curve, or a tangent plane on a solid. We can think of this as the set of all possible derivatives of the manifold at that point.\n",
    "\n",
    "The tangent space of $\\bf{SO}(3)$ lies in $\\bf{so}(3)$ which are the Lie algebras."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The tangent space at the identity is described by the Lie algebra of the group, and the basis directions of the tangent space are called the generators of the group. There are three generators of $\\bf{SO}(3)$ each belonging to $\\bf{so}(3)$.\n",
    "\n",
    "The three generators each provide a basis – which can be thought of as the $x$, $y$, and $z$ axes – for the group $\\bf{SO}(3)$.\n",
    "\n",
    "The generator for the x-axis is\n",
    "\\begin{equation*}\n",
    "    g_x =\n",
    "    \\begin{pmatrix}\n",
    "        0 & 0 & 0 \\\\\n",
    "        0 & 0 & -1 \\\\\n",
    "        0 & 1 & 0\n",
    "    \\end{pmatrix}.\n",
    "\\end{equation*}\n",
    "\n",
    "The generator for the y-axis is\n",
    "\\begin{equation*}\n",
    "    g_y =\n",
    "    \\begin{pmatrix}\n",
    "        0 & 0 & 1 \\\\\n",
    "        0 & 0 & 0 \\\\\n",
    "        -1 & 0 & 0\n",
    "    \\end{pmatrix}.\n",
    "\\end{equation*}\n",
    "\n",
    "The generator for the z-axis is\n",
    "\\begin{equation*}\n",
    "    g_z =\n",
    "    \\begin{pmatrix}\n",
    "        0 & -1 & 0 \\\\\n",
    "        1 & 0 & 0 \\\\\n",
    "        0 & 0 & 0\n",
    "    \\end{pmatrix}.\n",
    "\\end{equation*}"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Every point in the tangent space $\\bf{so}(3)$ – the derivatives of the manifold $\\bf{SO}(3)$ – can be expressed as a linear combination of basis matrices\n",
    "\n",
    "\\begin{equation*}\n",
    "    \\Omega(\\eta_1, \\ \\eta_2, \\ \\eta_3) =\n",
    "    \\eta_1 \\ g_x +\n",
    "    \\eta_2 \\ g_y +\n",
    "    \\eta_3 \\ g_z\n",
    "\\end{equation*}\n",
    "where $\\bf{\\eta} = (\\eta_1, \\ \\eta_2, \\ \\eta_3)$ are the exponential coordinates (also known as the Euler vector).\n",
    "\n",
    "Together, the exponential coordinates define an axis and an angle\n",
    "\n",
    "\\begin{equation*}\n",
    "    \\bf{\\eta} =\n",
    "    \\hat{\\eta} \\ \\theta\n",
    "\\end{equation*}\n",
    "where $\\hat{\\eta} \\in \\mathbb{R}^3$ is a unit vector which defines the axis and $\\theta$ defines the angle or rotation around the axis.\n",
    "\n",
    "Therefore $\\eta$ describes four numbers with\n",
    "\\begin{equation*}\n",
    "    \\theta = \\lvert\\lvert \\eta \\rvert\\rvert\n",
    "\\end{equation*}\n",
    "and \n",
    "\\begin{equation*}\n",
    "    \\hat{\\eta} = \\dfrac{\\eta}{\\lvert\\lvert \\eta \\rvert\\rvert}\n",
    "\\end{equation*}\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle g_x = \\left[\\begin{matrix}0 & 0 & 0\\\\0 & 0 & -1\\\\0 & 1 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle g_y = \\left[\\begin{matrix}0 & 0 & 1\\\\0 & 0 & 0\\\\-1 & 0 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle g_z = \\left[\\begin{matrix}0 & -1 & 0\\\\1 & 0 & 0\\\\0 & 0 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Lets make the basis matrices in Python\n",
    "gx = smb.skew([1, 0, 0])\n",
    "gy = smb.skew([0, 1, 0])\n",
    "gz = smb.skew([0, 0, 1])\n",
    "\n",
    "# Visualise the results\n",
    "ds.display(ds.Math(f\"g_x = {sym.latex(sym.Matrix(gx))}\"))\n",
    "ds.display(ds.Math(f\"g_y = {sym.latex(sym.Matrix(gy))}\"))\n",
    "ds.display(ds.Math(f\"g_z = {sym.latex(sym.Matrix(gz))}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We have the following relationship between the Lie algebra $\\phi$ and Lie group $\\Phi$\n",
    "\n",
    "\\begin{equation*}\n",
    "    \\forall \\bf{X} \\in \\bf{\\phi} \\Rightarrow e^{\\bf{X}} \\in \\bf{\\Phi}\n",
    "\\end{equation*}\n",
    "which says that the exponential of any member of $\\bf{so}(3)$ is a valid member of $\\bf{SO}(3)$\n",
    "\n",
    "Lets put this to the test in Python.\n",
    "\n",
    "We will use the Spatialmath Package to produce a rotation in $\\bf{SO}(3)$ which is around the x-axis by an amount $\\pi$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle R_x(\\pi) = \\left[\\begin{matrix}1.0 & 0 & 0\\\\0 & -1.0 & -1.22464679914735 \\cdot 10^{-16}\\\\0 & 1.22464679914735 \\cdot 10^{-16} & -1.0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "rot_x = sm.SO3.Rx(np.pi).A\n",
    "\n",
    "# View the result\n",
    "ds.display(ds.Math(f\"R_x(\\pi) = {sym.latex(sym.Matrix(rot_x))}\"))\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now we we get the same rotation using the theory detailed out above\n",
    "\n",
    "We will perform the equation\n",
    "\\begin{align*}\n",
    "    R_x(\\theta) &= e^{\\theta \\ g_x} \\\\\n",
    "    R_x(\\pi) &= e^{\\pi \\ g_x}\n",
    "\\end{align*}"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle R_x(\\pi) = \\left[\\begin{matrix}1.0 & 0 & 0\\\\0 & -1.0 & -1.52695104473598 \\cdot 10^{-16}\\\\0 & 1.52695104473598 \\cdot 10^{-16} & -1.0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "rot_x = expm(np.pi * gx)\n",
    "\n",
    "# View the result\n",
    "ds.display(ds.Math(f\"R_x(\\pi) = {sym.latex(sym.Matrix(rot_x))}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "which shows that we get the same result."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='so31'></a>\n",
    "\n",
    "### 3.2 Special Orthogonal Group – $\\bf{SO}(3)$ \n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can create every rotation in $\\bf{SO}(3)$ with the generators\n",
    "\n",
    "\\begin{align*}\n",
    "    \\Omega(\\bf\\eta)\n",
    "    &= \n",
    "    \\begin{pmatrix}\n",
    "        0 & -\\eta_3 & \\eta_2 \\\\\n",
    "        \\eta_3 & 0 & -\\eta_1 \\\\\n",
    "        -\\eta_2 & \\eta_1 & 0\n",
    "    \\end{pmatrix} \\\\\n",
    "    &= \n",
    "    [\\eta]_\\times\n",
    "\\end{align*}\n",
    "where $[\\cdot]_\\times : \\mathbb{R}^3 \\mapsto \\bf{so}(3)$ maps a vector to a skew symmetric matrix.\n",
    "\n",
    "We then create a rotation with\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{R}(\\bf\\eta)\n",
    "    &= e^{[\\eta]_\\times} \\\\\n",
    "    &= e^{[\\hat{\\eta}]_\\times \\ \\theta} \\\\\n",
    "\\end{align*}\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Coming up with the axis and angle to define $\\eta$ can be tricky in practice. Another strategy can be to construct rotations using a combination of the generators.\n",
    "\n",
    "For example, we can construct a rotation made from roll-pitch-yaw angles applied in the xyz order:\n",
    "\\begin{align*}\n",
    "    \\bf{R}(\\alpha, \\ \\beta, \\ \\gamma)\n",
    "    &= \n",
    "    e^{\\gamma \\ g_x} \\ e^{\\beta \\ g_y} \\ e^{\\alpha \\ g_y}\n",
    "\\end{align*}"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Lets view a rotation with rpy angles $\\Gamma = (\\frac{\\pi}{2}, \\ \\frac{\\pi}{2}, \\ \\frac{\\pi}{2})$ made using the Spatialmaths package"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle R = \\left[\\begin{matrix}3.74939945665464 \\cdot 10^{-33} & -6.12323399573677 \\cdot 10^{-17} & 1.0\\\\1.22464679914735 \\cdot 10^{-16} & -1.0 & -6.12323399573677 \\cdot 10^{-17}\\\\1.0 & 1.22464679914735 \\cdot 10^{-16} & 3.74939945665464 \\cdot 10^{-33}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "rot_rpy = sm.SO3.RPY([np.pi/2, np.pi/2, np.pi/2], order='xyz').A\n",
    "\n",
    "# View the result\n",
    "ds.display(ds.Math(f\"R = {sym.latex(sym.Matrix(rot_rpy))}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now lets try using the generators"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle R = \\left[\\begin{matrix}0 & 0 & 1.0\\\\0 & -1.0 & 0\\\\1.0 & 0 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "rot_rpy = expm(np.pi/2 * gx) @ expm(np.pi/2 * gy) @ expm(np.pi/2 * gz)\n",
    "\n",
    "# View the result\n",
    "ds.display(ds.Math(f\"R = {sym.latex(sym.Matrix(rot_rpy))}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='so3d'></a>\n",
    "\n",
    "### 3.3 Derivatives of $\\bf{SO}(3)$ and the Angular Jacobian\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this section, we are going to define a new Jacobian which relates some angular rate unit to angular velocity. In this case we are choosing xyz roll-pitch-yaw angle rates. Using the same techniques presented in this Notebook, you can create a new Jacobian to relate different angular rate units if desired."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We have the following formula for a member of $\\bf{SO}(3)$\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{R}(\\bf\\eta)\n",
    "    &= e^{[\\eta]_\\times} \\\\\n",
    "\\end{align*}\n",
    "\n",
    "Using the chain rule, we can take the derivative\n",
    "\n",
    "\\begin{align*}\n",
    "    \\dot{\\bf{R}}\n",
    "    &= \n",
    "    \\dfrac{\\mathrm{d}\\ {[\\eta]_\\times}}{\\mathrm{d} t}\n",
    "    \\ \\\n",
    "    e^{[\\eta]_\\times} \\\\\n",
    "    &= \n",
    "    [\\omega]_\\times \\\n",
    "    e^{[\\eta]_\\times} \\\\\n",
    "    &= \n",
    "    [\\omega]_\\times \\\n",
    "    \\bf{R}\n",
    "\\end{align*}\n",
    "where $\\omega = (\\omega_x, \\ \\omega_y, \\ \\omega_z)$ describes the angular velocity. In a similar theme to $\\eta$, the angular velocity describes an axis of rotation, and a rate of rotation about that axis."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To test some derivatives, lets first make some symbolic variables in Python using the `sympy` package. These variables will define some roll-pitch-yaw angles $\\Gamma = (\\alpha, \\ \\beta, \\ \\gamma)$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle Γ = \\left[ α{\\left(t \\right)}, \\  β{\\left(t \\right)}, \\  γ{\\left(t \\right)}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# A variable for time\n",
    "t = sym.symbols('t')\n",
    "\n",
    "# Variables for roll, pitch and yaw\n",
    "α, β, γ = sym.symbols(\"α β γ\")\n",
    "\n",
    "# Make the angles functions of time\n",
    "αt = sym.Function(α)(t)\n",
    "βt = sym.Function(β)(t)\n",
    "γt = sym.Function(γ)(t)\n",
    "\n",
    "# List to hold python friendly values\n",
    "Γ_py = [α, β, γ]\n",
    "\n",
    "# List to hold the angles\n",
    "Γ = [αt, βt, γt]\n",
    "\n",
    "# View the results\n",
    "ds.display(ds.Math(f\"Γ = {sym.latex(Γ)}\"))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\dot{Γ} = \\left[ \\dot{α}, \\  \\dot{β}, \\  \\dot{γ}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\ddot{Γ} = \\left[ \\ddot{α}, \\  \\ddot{β}, \\  \\ddot{γ}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Now lets makes the derivatives of those angles\n",
    "# These variables represent the roll, pitch, and yaw rates\n",
    "αdt = sym.diff(αt)\n",
    "βdt = sym.diff(βt)\n",
    "γdt = sym.diff(γt)\n",
    "Γd_py = [str(a) + \"d\" for a in Γ_py]\n",
    "Γd = [αdt, βdt, γdt]\n",
    "\n",
    "# Now lets makes the derivatives of those velocities\n",
    "# These variables represent the roll, pitch, and yaw accelerations\n",
    "αddt = sym.diff(αdt)\n",
    "βddt = sym.diff(βdt)\n",
    "γddt = sym.diff(γdt)\n",
    "Γdd_py = [str(a) + \"d\" for a in Γd_py]\n",
    "Γdd = [αddt, βddt, γddt]\n",
    "\n",
    "# View the results\n",
    "ds.display(ds.Math(f\"\\dot{{Γ}} = {vlatex(Γd)}\"))\n",
    "ds.display(ds.Math(f\"\\ddot{{Γ}} = {vlatex(Γdd)}\"))\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The generators are not the only way to construct a rotation. We can use a known identity to construct a rotation around the x, y, or z-axis.\n",
    "\n",
    "These identities are\n",
    "\n",
    "$$\n",
    "    \\bf{R}_x(\\theta) = \n",
    "    \\left[\\begin{matrix}1 & 0 & 0\\\\0 & \\cos{\\left(\\theta \\right)} & - \\sin{\\left(\\theta \\right)}\\\\0 & \\sin{\\left(\\theta \\right)} & \\cos{\\left(\\theta \\right)}\\end{matrix}\\right]\n",
    "$$\n",
    "\n",
    "$$\n",
    "    \\bf{R}_y(\\theta) = \n",
    "    \\left[\\begin{matrix}\\cos{\\left(\\theta \\right)} & 0 & \\sin{\\left(\\theta \\right)}\\\\0 & 1 & 0\\\\- \\sin{\\left(\\theta \\right)} & 0 & \\cos{\\left(\\theta \\right)}\\end{matrix}\\right]\n",
    "$$\n",
    "\n",
    "$$\n",
    "    \\bf{R}_z(\\theta) = \n",
    "    \\left[\\begin{matrix}\\cos{\\left(\\theta \\right)} & - \\sin{\\left(\\theta \\right)} & 0\\\\\\sin{\\left(\\theta \\right)} & \\cos{\\left(\\theta \\right)} & 0\\\\0 & 0 & 1\\end{matrix}\\right]\n",
    "$$\n",
    "\n",
    "The Spatialmath package uses these identities to construct $\\textbf{SO}(3)$ objects.\n",
    "\n",
    "For example:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle R_x(θ) = \\left[\\begin{matrix}1 & 0 & 0\\\\0 & \\cos{\\left(θ \\right)} & - \\sin{\\left(θ \\right)}\\\\0 & \\sin{\\left(θ \\right)} & \\cos{\\left(θ \\right)}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle R_y(θ) = \\left[\\begin{matrix}\\cos{\\left(θ \\right)} & 0 & \\sin{\\left(θ \\right)}\\\\0 & 1 & 0\\\\- \\sin{\\left(θ \\right)} & 0 & \\cos{\\left(θ \\right)}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle R_z(θ) = \\left[\\begin{matrix}\\cos{\\left(θ \\right)} & - \\sin{\\left(θ \\right)} & 0\\\\\\sin{\\left(θ \\right)} & \\cos{\\left(θ \\right)} & 0\\\\0 & 0 & 1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Make a theta symbolic variable\n",
    "θ = sym.symbols('θ')\n",
    "\n",
    "# Make the rotations using the spatialmaths package\n",
    "rot_x = sm.SO3.Rx(θ).A\n",
    "rot_y = sm.SO3.Ry(θ).A\n",
    "rot_z = sm.SO3.Rz(θ).A\n",
    "\n",
    "# View the results, these should match the equations above\n",
    "ds.display(ds.Math(f\"R_x(θ) = {sym.latex(sym.Matrix(rot_x))}\"))\n",
    "ds.display(ds.Math(f\"R_y(θ) = {sym.latex(sym.Matrix(rot_y))}\"))\n",
    "ds.display(ds.Math(f\"R_z(θ) = {sym.latex(sym.Matrix(rot_z))}\"))\n",
    "\n",
    "# print(sym.latex(sym.Matrix(rot_x), mode='equation*'))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As we showed previously, a rotation can be constructed by applying combinations of elementary rotations. A rotation can be constructed from roll, pitch, and yaw angles and an order. The order defines which rotations correspond to which rotation.\n",
    "\n",
    "For example, for zyx roll-pitch-yaw angles, where zyx is the order, and the angles are $\\Gamma = (\\alpha, \\ \\beta, \\ \\gamma)$ the resulting rotation is \n",
    "\n",
    "\\begin{equation*}\n",
    "    \\bf{R}(\\Gamma) = \n",
    "    \\bf{R}_z(\\gamma)\n",
    "    \\bf{R}_y(\\beta)\n",
    "    \\bf{R}_x(\\alpha)\n",
    "\\end{equation*}\n",
    "\n",
    "Another example, for xyz roll-pitch-yaw angles, the resulting rotation is \n",
    "\n",
    "\\begin{equation*}\n",
    "    \\bf{R}(\\Gamma) = \n",
    "    \\bf{R}_x(\\gamma)\n",
    "    \\bf{R}_y(\\beta)\n",
    "    \\bf{R}_y(\\alpha)\n",
    "\\end{equation*}\n",
    "\n",
    "We will stick with the xyz order for the rest of the this notebook.\n",
    "\n",
    "Lets make this rotation using Spatialmaths"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle R(Γ) = \\left[\\begin{matrix}\\cos{\\left(α{\\left(t \\right)} \\right)} \\cos{\\left(β{\\left(t \\right)} \\right)} & - \\sin{\\left(α{\\left(t \\right)} \\right)} \\cos{\\left(β{\\left(t \\right)} \\right)} & \\sin{\\left(β{\\left(t \\right)} \\right)}\\\\\\sin{\\left(α{\\left(t \\right)} \\right)} \\cos{\\left(γ{\\left(t \\right)} \\right)} + \\sin{\\left(β{\\left(t \\right)} \\right)} \\sin{\\left(γ{\\left(t \\right)} \\right)} \\cos{\\left(α{\\left(t \\right)} \\right)} & - \\sin{\\left(α{\\left(t \\right)} \\right)} \\sin{\\left(β{\\left(t \\right)} \\right)} \\sin{\\left(γ{\\left(t \\right)} \\right)} + \\cos{\\left(α{\\left(t \\right)} \\right)} \\cos{\\left(γ{\\left(t \\right)} \\right)} & - \\sin{\\left(γ{\\left(t \\right)} \\right)} \\cos{\\left(β{\\left(t \\right)} \\right)}\\\\\\sin{\\left(α{\\left(t \\right)} \\right)} \\sin{\\left(γ{\\left(t \\right)} \\right)} - \\sin{\\left(β{\\left(t \\right)} \\right)} \\cos{\\left(α{\\left(t \\right)} \\right)} \\cos{\\left(γ{\\left(t \\right)} \\right)} & \\sin{\\left(α{\\left(t \\right)} \\right)} \\sin{\\left(β{\\left(t \\right)} \\right)} \\cos{\\left(γ{\\left(t \\right)} \\right)} + \\sin{\\left(γ{\\left(t \\right)} \\right)} \\cos{\\left(α{\\left(t \\right)} \\right)} & \\cos{\\left(β{\\left(t \\right)} \\right)} \\cos{\\left(γ{\\left(t \\right)} \\right)}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Make the elementary rotations\n",
    "Rx = sm.SO3.Rx(γt).A\n",
    "Ry = sm.SO3.Ry(βt).A\n",
    "Rz = sm.SO3.Rz(αt).A\n",
    "\n",
    "# Make the xyz roll-pitch-yaw rotation\n",
    "R = Rx @ Ry @ Rz\n",
    "\n",
    "# View the result\n",
    "ds.display(ds.Math(f\"R(Γ) = {sym.latex(sym.Matrix(R))}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now lets take the derivative of $\\bf{R}(\\Gamma)$ using the product rule\n",
    "\n",
    "\\begin{equation*}\n",
    "    \\dot{\\bf{R}} = \n",
    "    \\dot{\\bf{R}_x}\n",
    "    \\bf{R}_y\n",
    "    \\bf{R}_y +\n",
    "    \\bf{R}_x\n",
    "    \\dot{\\bf{R}_y}\n",
    "    \\bf{R}_y +\n",
    "    \\bf{R}_x\n",
    "    \\bf{R}_y\n",
    "    \\dot{\\bf{R}_y}\n",
    "\\end{equation*}\n",
    "\n",
    "Now to work out $\\dot{\\bf{R}_x}$ lets look at the definition of $\\bf{R}$\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{R}_x(\\bf\\gamma)\n",
    "    &= e^{\\gamma \\ g_x} \\\\\n",
    "\\end{align*}\n",
    "\n",
    "and take the derivative using the chain rule as we did above\n",
    "\n",
    "\\begin{align*}\n",
    "    \\dot{\\bf{R}}_x\n",
    "    &= \n",
    "    \\dfrac{\\mathrm{d}\\ {\\gamma  g_x}}{\\mathrm{d} t}\n",
    "    \\ \\\n",
    "    e^{\\gamma  g_x} \\\\\n",
    "    &= \n",
    "    \\dot{\\gamma}  g_x \\\n",
    "    e^{\\gamma  g_x} \\\\\n",
    "    &= \n",
    "    \\dot{\\gamma}  g_x \\\n",
    "    \\bf{R}_x\n",
    "\\end{align*}\n",
    "\n",
    "Lets try this out in Python before going any further"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\dot{R}_x = \\left[\\begin{matrix}0 & 0 & 0\\\\0 & - \\sin{\\left(γ \\right)} \\dot{γ} & - \\cos{\\left(γ \\right)} \\dot{γ}\\\\0 & \\cos{\\left(γ \\right)} \\dot{γ} & - \\sin{\\left(γ \\right)} \\dot{γ}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Calculate the derivative of Rx\n",
    "dRx = (γdt * gx) @ Rx\n",
    "\n",
    "# View the result\n",
    "ds.display(ds.Math(f\"\\dot{{R}}_x = {vlatex(sym.Matrix(dRx))}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To validate our method, lets use `sympy` symbolic derivative function to compare results"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\dot{R}_x = \\left[\\begin{matrix}0 & 0 & 0\\\\0 & - \\sin{\\left(γ \\right)} \\dot{γ} & - \\cos{\\left(γ \\right)} \\dot{γ}\\\\0 & \\cos{\\left(γ \\right)} \\dot{γ} & - \\sin{\\left(γ \\right)} \\dot{γ}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Use symbolic differentiation\n",
    "dRx_sym = sym.diff(sym.Matrix(Rx))\n",
    "\n",
    "# View the sresult\n",
    "ds.display(ds.Math(f\"\\dot{{R}}_x = {vlatex(sym.Matrix(dRx_sym))}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Lets calculate the derivatives for Ry and Rz in the same manner before going any further"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\dot{R}_y = \\left[\\begin{matrix}- \\sin{\\left(β \\right)} \\dot{β} & 0 & \\cos{\\left(β \\right)} \\dot{β}\\\\0 & 0 & 0\\\\- \\cos{\\left(β \\right)} \\dot{β} & 0 & - \\sin{\\left(β \\right)} \\dot{β}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\dot{R}_z = \\left[\\begin{matrix}- \\sin{\\left(α \\right)} \\dot{α} & - \\cos{\\left(α \\right)} \\dot{α} & 0\\\\\\cos{\\left(α \\right)} \\dot{α} & - \\sin{\\left(α \\right)} \\dot{α} & 0\\\\0 & 0 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Calculate the derivative of Ry\n",
    "dRy = (βdt * gy) @ Ry\n",
    "dRz = (αdt * gz) @ Rz\n",
    "\n",
    "# View the result\n",
    "ds.display(ds.Math(f\"\\dot{{R}}_y = {vlatex(sym.Matrix(dRy))}\"))\n",
    "ds.display(ds.Math(f\"\\dot{{R}}_z = {vlatex(sym.Matrix(dRz))}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now recall our derivative of $\\bf{R}(\\Gamma)$ previously defined\n",
    "\n",
    "\\begin{equation*}\n",
    "    \\dot{\\bf{R}} = \n",
    "    \\dot{\\bf{R}_x}\n",
    "    \\bf{R}_y\n",
    "    \\bf{R}_y +\n",
    "    \\bf{R}_x\n",
    "    \\dot{\\bf{R}_y}\n",
    "    \\bf{R}_y +\n",
    "    \\bf{R}_x\n",
    "    \\bf{R}_y\n",
    "    \\dot{\\bf{R}_y}\n",
    "\\end{equation*}\n",
    "\n",
    "While we could simplify this further, this will do for the purpose of this tutorial.\n",
    "\n",
    "We now have the necessary components to calculate this in Python"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\dot{R} = \\left[\\begin{matrix}- \\sin{\\left(α \\right)} \\cos{\\left(β \\right)} \\dot{α} - \\sin{\\left(β \\right)} \\cos{\\left(α \\right)} \\dot{β} & \\sin{\\left(α \\right)} \\sin{\\left(β \\right)} \\dot{β} - \\cos{\\left(α \\right)} \\cos{\\left(β \\right)} \\dot{α} & \\cos{\\left(β \\right)} \\dot{β}\\\\- \\sin{\\left(α \\right)} \\sin{\\left(β \\right)} \\sin{\\left(γ \\right)} \\dot{α} - \\sin{\\left(α \\right)} \\sin{\\left(γ \\right)} \\dot{γ} + \\sin{\\left(β \\right)} \\cos{\\left(α \\right)} \\cos{\\left(γ \\right)} \\dot{γ} + \\sin{\\left(γ \\right)} \\cos{\\left(α \\right)} \\cos{\\left(β \\right)} \\dot{β} + \\cos{\\left(α \\right)} \\cos{\\left(γ \\right)} \\dot{α} & - \\sin{\\left(α \\right)} \\sin{\\left(β \\right)} \\cos{\\left(γ \\right)} \\dot{γ} - \\sin{\\left(α \\right)} \\sin{\\left(γ \\right)} \\cos{\\left(β \\right)} \\dot{β} - \\sin{\\left(α \\right)} \\cos{\\left(γ \\right)} \\dot{α} - \\sin{\\left(β \\right)} \\sin{\\left(γ \\right)} \\cos{\\left(α \\right)} \\dot{α} - \\sin{\\left(γ \\right)} \\cos{\\left(α \\right)} \\dot{γ} & \\sin{\\left(β \\right)} \\sin{\\left(γ \\right)} \\dot{β} - \\cos{\\left(β \\right)} \\cos{\\left(γ \\right)} \\dot{γ}\\\\\\sin{\\left(α \\right)} \\sin{\\left(β \\right)} \\cos{\\left(γ \\right)} \\dot{α} + \\sin{\\left(α \\right)} \\cos{\\left(γ \\right)} \\dot{γ} + \\sin{\\left(β \\right)} \\sin{\\left(γ \\right)} \\cos{\\left(α \\right)} \\dot{γ} + \\sin{\\left(γ \\right)} \\cos{\\left(α \\right)} \\dot{α} - \\cos{\\left(α \\right)} \\cos{\\left(β \\right)} \\cos{\\left(γ \\right)} \\dot{β} & - \\sin{\\left(α \\right)} \\sin{\\left(β \\right)} \\sin{\\left(γ \\right)} \\dot{γ} - \\sin{\\left(α \\right)} \\sin{\\left(γ \\right)} \\dot{α} + \\sin{\\left(α \\right)} \\cos{\\left(β \\right)} \\cos{\\left(γ \\right)} \\dot{β} + \\sin{\\left(β \\right)} \\cos{\\left(α \\right)} \\cos{\\left(γ \\right)} \\dot{α} + \\cos{\\left(α \\right)} \\cos{\\left(γ \\right)} \\dot{γ} & - \\sin{\\left(β \\right)} \\cos{\\left(γ \\right)} \\dot{β} - \\sin{\\left(γ \\right)} \\cos{\\left(β \\right)} \\dot{γ}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "dR = dRx @ Ry @ Rz + Rx @ dRy @ Rz + Rx @ Ry @ dRz\n",
    "\n",
    "# View the result\n",
    "ds.display(ds.Math(f\"\\dot{{R}} = {vlatex(sym.Matrix(dR))}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now using the identity we derived previously\n",
    "\\begin{align*}\n",
    "    \\dot{\\bf{R}}\n",
    "    &= \n",
    "    [\\omega]_\\times \\\n",
    "    \\bf{R}\n",
    "\\end{align*}\n",
    "\n",
    "we can rearrange to obtain the angular velocity $\\omega$\n",
    "\n",
    "\\begin{align*}\n",
    "    \\dot{\\bf{R}} \\bf{R}^\\top\n",
    "    &= \n",
    "    [\\omega]_\\times \\\\\n",
    "    \\vee_\\times \\left( \\dot{\\bf{R}} \\bf{R}^\\top \\right)\n",
    "    &= \n",
    "    \\omega\n",
    "\\end{align*}\n",
    "\n",
    "Now lets try in Python"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle ω = \\left[\\begin{matrix}\\sin{\\left(β \\right)} \\dot{α} + \\dot{γ}\\\\- \\sin{\\left(γ \\right)} \\cos{\\left(β \\right)} \\dot{α} + \\cos{\\left(γ \\right)} \\dot{β}\\\\\\sin{\\left(γ \\right)} \\dot{β} + \\cos{\\left(β \\right)} \\cos{\\left(γ \\right)} \\dot{α}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "ω = vex(dR @ R.T)\n",
    "\n",
    "# Simplify the result to make it more readable\n",
    "ω = sym.simplify(ω)\n",
    "\n",
    "# View the result\n",
    "ds.display(ds.Math(f\"ω = {vlatex(sym.Matrix(ω))}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We now have a relationship between angular velocity and roll-pitch-yaw rates.\n",
    "\n",
    "Lets factorise the roll-pitch-yaw rates out of the above result"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle A = \\left[\\begin{matrix}\\sin{\\left(β \\right)} & 0 & 1\\\\- \\sin{\\left(γ \\right)} \\cos{\\left(β \\right)} & \\cos{\\left(γ \\right)} & 0\\\\\\cos{\\left(β \\right)} \\cos{\\left(γ \\right)} & \\sin{\\left(γ \\right)} & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Lets factorise the result to extract the roll, pitch and yaw rates\n",
    "A = sym.Matrix.zeros(3,3)\n",
    "for i in range(3):\n",
    "    e = ω[i].expand()\n",
    "    for j in range(3):\n",
    "        A[i, j] = e.coeff(Γd[j])\n",
    "\n",
    "# View the result\n",
    "ds.display(ds.Math(f\"A = {vlatex(sym.Matrix(A))}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now we have the matrix $A$. This is a Jacobian which in this case relates angular velocity to xyz roll-pitch-yaw rates\n",
    "\n",
    "\\begin{align*}\n",
    "    \\omega =\n",
    "    \\bf{A}(\\Gamma)\n",
    "    \\dot{\\Gamma}\n",
    "\\end{align*}"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle ω = \\left[\\begin{matrix}\\sin{\\left(β \\right)} & 0 & 1\\\\- \\sin{\\left(γ \\right)} \\cos{\\left(β \\right)} & \\cos{\\left(γ \\right)} & 0\\\\\\cos{\\left(β \\right)} \\cos{\\left(γ \\right)} & \\sin{\\left(γ \\right)} & 0\\end{matrix}\\right]\\left[\\begin{matrix}\\dot{α}\\\\\\dot{β}\\\\\\dot{γ}\\end{matrix}\\right] $"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# View in expanded form in Python\n",
    "ds.display(ds.Math(f\"ω = {vlatex(sym.Matrix(A))}{vlatex(sym.Matrix(Γd))} \"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This process shows how we get to Equation (31) in the Tutorial. "
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We will first create the $\\bf{A}^{-1}$ matrix"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle A^{-1} = \\left[\\begin{matrix}0 & - \\frac{\\sin{\\left(γ \\right)}}{\\cos{\\left(β \\right)}} & \\frac{\\cos{\\left(γ \\right)}}{\\cos{\\left(β \\right)}}\\\\0 & \\cos{\\left(γ \\right)} & \\sin{\\left(γ \\right)}\\\\1 & \\sin{\\left(γ \\right)} \\tan{\\left(β \\right)} & - \\cos{\\left(γ \\right)} \\tan{\\left(β \\right)}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "Ai = sym.trigsimp(A.inv())\n",
    "\n",
    "# View the result\n",
    "ds.display(ds.Math(f\"A^{{-1}} = {vlatex(sym.Matrix(Ai))}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Lets turn $\\bf{A}$ and $\\bf{A}^{-1}$ into Python code"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The A matrix: np.array([[math.sin(β), 0, 1], [-math.sin(γ)*math.cos(β), math.cos(γ), 0], [math.cos(β)*math.cos(γ), math.sin(γ), 0]])\n",
      "The Ai matrix: np.array([[0, -math.sin(γ)/math.cos(β), math.cos(γ)/math.cos(β)], [0, math.cos(γ), math.sin(γ)], [1, math.sin(γ)*math.tan(β), -math.cos(γ)*math.tan(β)]])\n"
     ]
    }
   ],
   "source": [
    "py_A = sym.pycode(A.subs(a for a in zip(Γ, Γ_py))).replace('ImmutableDenseMatrix', 'np.array')\n",
    "py_Ai = sym.pycode(Ai.subs(a for a in zip(Γ, Γ_py))).replace('ImmutableDenseMatrix', 'np.array')\n",
    "\n",
    "# View the results\n",
    "print(f\"The A matrix: {py_A}\")\n",
    "print(f\"The Ai matrix: {py_Ai}\")\n",
    "\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can then copy and paste these into Python methods"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [],
   "source": [
    "def JA(Γ):\n",
    "    \"\"\"\n",
    "    Returns a Jacobian which relates angular velocity to\n",
    "    xyz roll-pitch-yaw rates.\n",
    "\n",
    "    Γ is a 3 vector representing the current RPY angles\n",
    "    \"\"\"\n",
    "    α, β, γ = Γ\n",
    "\n",
    "    return np.array([\n",
    "        [math.sin(β), 0, 1],\n",
    "        [-math.sin(γ)*math.cos(β), math.cos(γ), 0],\n",
    "        [math.cos(β)*math.cos(γ), math.sin(γ), 0]\n",
    "    ])\n",
    "\n",
    "def JAi(Γ):\n",
    "    \"\"\"\n",
    "    Returns a Jacobian which relates xyz roll-pitch-yaw rates\n",
    "    to angular velocity.\n",
    "\n",
    "    Γ is a 3 vector representing the current RPY angles\n",
    "    \"\"\"\n",
    "    α, β, γ = Γ\n",
    "\n",
    "    return np.array([\n",
    "        [0, -math.sin(γ)/math.cos(β), math.cos(γ)/math.cos(β)],\n",
    "        [0, math.cos(γ), math.sin(γ)],\n",
    "        [1, math.sin(γ)*math.tan(β), -math.cos(γ)*math.tan(β)]\n",
    "    ])"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Lets test these methods out"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle ω = \\left[\\begin{matrix}0.28\\\\-0.04\\\\-0.21\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\dot{Γ} = \\left[\\begin{matrix}-0.2\\\\-0.1\\\\0.2\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Set up our example xyz roll-pitch-yaw vector\n",
    "ex_Γ = [0.1, -0.4, 0.3]\n",
    "\n",
    "# Set up our example xyz roll-pitch-yaw rate vector\n",
    "ex_Γd = [-0.2, -0.1, 0.2]\n",
    "\n",
    "# Using the JA method, we can convert roll-pitch-yaw rates into\n",
    "# angular velocity\n",
    "ex_ω = JA(ex_Γ) @ ex_Γd\n",
    "\n",
    "# Using the JAi method, we can convert the angular velocity\n",
    "# back into roll-pitch-yaw rates\n",
    "ex_Γd_rev = JAi(ex_Γ) @ ex_ω\n",
    "\n",
    "# View the result\n",
    "ds.display(ds.Math(f\"ω = {vlatex(sym.Matrix(np.round(ex_ω, 2)))}\"))\n",
    "ds.display(ds.Math(f\"\\dot{{Γ}} = {vlatex(sym.Matrix(np.round(ex_Γd_rev, 2)))}\"))\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='acc'></a>\n",
    "\n",
    "### 3.4 Second Derivatives of $\\bf{SO}(3)$ and the Angular Hessian\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this section, we are going to define a method which relates some angular acceleration unit to angular velocity. In this case we are choosing xyz roll-pitch-yaw angle accelerations. Using the same techniques presented in this Notebook, you can modify this section to relate different angular acceleration units if desired."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We have the following formula for the derivative of an $\\bf{SO}(3)$ object\n",
    "\n",
    "\\begin{align*}\n",
    "    \\dot{\\bf{R}}\n",
    "    &=\n",
    "    [\\omega]_\\times \\\n",
    "    \\bf{R}\n",
    "\\end{align*}\n",
    "\n",
    "Using the product and chain rule, we can take the second derivative\n",
    "\n",
    "\\begin{align*}\n",
    "    \\ddot{\\bf{R}}\n",
    "    &= \n",
    "    [\\dot{\\omega}]_\\times \\\n",
    "    [\\omega]_\\times \\\n",
    "    e^{[\\eta]_\\times}\n",
    "    +\n",
    "    [\\omega]_\\times \\\n",
    "    [\\omega]_\\times \\\n",
    "    e^{[\\eta]_\\times}\n",
    "    \\\\\n",
    "    &= \n",
    "    [\\alpha]_\\times \\\n",
    "    \\bf{R}\n",
    "    +\n",
    "    [\\omega]_\\times \\\n",
    "    \\dot{\\bf{R}}\n",
    "\\end{align*}\n",
    "where we have unfortunately doubled up on our use of $\\alpha$ (previously used in the roll-pitch-yaw vector $\\Gamma$). In this context $\\alpha = (\\alpha_x, \\ \\alpha_y, \\ \\alpha_z)$ describes the angular acceleration. In a similar theme to $\\omega$, the angular acceleration describes an axis of rotation, and acceleration of rotation about that axis."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As we did for the first derivative, lets take the derivative of $\\dot{\\bf{R}}$ using the xyz roll-pitch-yaw definition\n",
    "\n",
    "\\begin{align*}\n",
    "    \\ddot{\\bf{R}}\n",
    "    &= \n",
    "    \\ddot{\\bf{R}_x}\n",
    "    \\bf{R}_y\n",
    "    \\bf{R}_y\n",
    "    +\n",
    "    \\dot{\\bf{R}_x}\n",
    "    \\dot{\\bf{R}_y}\n",
    "    \\bf{R}_y\n",
    "    +\n",
    "    \\dot{\\bf{R}_x}\n",
    "    \\bf{R}_y\n",
    "    \\dot{\\bf{R}_y} \\\\\n",
    "    %\n",
    "    & \\quad +\n",
    "    %\n",
    "    \\dot{\\bf{R}_x}\n",
    "    \\dot{\\bf{R}_y}\n",
    "    \\bf{R}_y\n",
    "    +\n",
    "    \\bf{R}_x\n",
    "    \\ddot{\\bf{R}_y}\n",
    "    \\bf{R}_y\n",
    "    +\n",
    "    \\bf{R}_x\n",
    "    \\dot{\\bf{R}_y}\n",
    "    \\dot{\\bf{R}_y} \\\\\n",
    "    %\n",
    "    & \\quad +\n",
    "    %\n",
    "    \\dot{\\bf{R}_x}\n",
    "    \\bf{R}_y\n",
    "    \\dot{\\bf{R}_y}\n",
    "    +\n",
    "    \\bf{R}_x\n",
    "    \\dot{\\bf{R}_y}\n",
    "    \\dot{\\bf{R}_y}\n",
    "    +\n",
    "    \\bf{R}_x\n",
    "    \\bf{R}_y\n",
    "    \\ddot{\\bf{R}_y} \\\\\n",
    "    &=\n",
    "    \\ddot{\\bf{R}_x}\n",
    "    \\bf{R}_y\n",
    "    \\bf{R}_y\n",
    "    +\n",
    "    \\bf{R}_x\n",
    "    \\ddot{\\bf{R}_y}\n",
    "    \\bf{R}_y\n",
    "    +\n",
    "    \\bf{R}_x\n",
    "    \\bf{R}_y\n",
    "    \\ddot{\\bf{R}_y} +\n",
    "    2 \\left(\n",
    "    \\dot{\\bf{R}_x}\n",
    "    \\dot{\\bf{R}_y}\n",
    "    \\bf{R}_y\n",
    "    +\n",
    "    \\dot{\\bf{R}_x}\n",
    "    \\bf{R}_y\n",
    "    \\dot{\\bf{R}_y}\n",
    "    +\n",
    "    \\bf{R}_x\n",
    "    \\dot{\\bf{R}_y}\n",
    "    \\dot{\\bf{R}_y}\n",
    "    \\right)\n",
    "\\end{align*}"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now to work out $\\ddot{\\bf{R}_x}$ lets look at the definition of $\\dot{\\bf{R}}$\n",
    "\n",
    "\\begin{align*}\n",
    "    \\dot{\\bf{R}}_x\n",
    "    &= \n",
    "    \\dot{\\gamma} g_x \\\n",
    "    e^{\\gamma  g_x} \\\\\n",
    "\\end{align*}\n",
    "\n",
    "and take the second derivative using the product and chain rule\n",
    "\n",
    "\\begin{align*}\n",
    "    \\ddot{\\bf{R}}_x\n",
    "    &= \n",
    "    \\ddot{\\gamma} g_x \\\n",
    "    e^{\\gamma  g_x} \n",
    "    +\n",
    "    \\dot{\\gamma} g_x \\\n",
    "    \\dot{\\gamma} g_x \\\n",
    "    e^{\\gamma \\ g_x} \\\\\n",
    "    &= \n",
    "    \\ddot{\\gamma} g_x \\\n",
    "    \\bf{R}_x\n",
    "    +\n",
    "    \\dot{\\gamma} g_x \\\n",
    "    \\dot{\\bf{R}_x}\n",
    "\\end{align*}\n",
    "\n",
    "Lets try this out in Python before going any further"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\ddot{R}_x = \\left[\\begin{matrix}0 & 0 & 0\\\\0 & - \\sin{\\left(γ \\right)} \\ddot{γ} - \\cos{\\left(γ \\right)} \\dot{γ}^{2} & \\sin{\\left(γ \\right)} \\dot{γ}^{2} - \\cos{\\left(γ \\right)} \\ddot{γ}\\\\0 & - \\sin{\\left(γ \\right)} \\dot{γ}^{2} + \\cos{\\left(γ \\right)} \\ddot{γ} & - \\sin{\\left(γ \\right)} \\ddot{γ} - \\cos{\\left(γ \\right)} \\dot{γ}^{2}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Calculate the derivative of Rx\n",
    "ddRx = (γddt * gx) @ Rx + (γdt * gx) @ dRx\n",
    "\n",
    "# View the result\n",
    "ds.display(ds.Math(f\"\\ddot{{R}}_x = {vlatex(sym.Matrix(ddRx))}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To validate our method, lets use `sympy` symbolic derivative function to compare results"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\ddot{R}_x = \\left[\\begin{matrix}0 & 0 & 0\\\\0 & - \\sin{\\left(γ \\right)} \\ddot{γ} - \\cos{\\left(γ \\right)} \\dot{γ}^{2} & \\sin{\\left(γ \\right)} \\dot{γ}^{2} - \\cos{\\left(γ \\right)} \\ddot{γ}\\\\0 & - \\sin{\\left(γ \\right)} \\dot{γ}^{2} + \\cos{\\left(γ \\right)} \\ddot{γ} & - \\sin{\\left(γ \\right)} \\ddot{γ} - \\cos{\\left(γ \\right)} \\dot{γ}^{2}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Use symbolic differentiation\n",
    "ddRx_sym = sym.diff(sym.Matrix(dRx))\n",
    "\n",
    "# View the sresult\n",
    "ds.display(ds.Math(f\"\\ddot{{R}}_x = {vlatex(sym.Matrix(ddRx_sym))}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Lets calculate the derivatives for Ry and Rz in the same manner before going any further"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\ddot{R}_y = \\left[\\begin{matrix}- \\sin{\\left(β \\right)} \\ddot{β} - \\cos{\\left(β \\right)} \\dot{β}^{2} & 0 & - \\sin{\\left(β \\right)} \\dot{β}^{2} + \\cos{\\left(β \\right)} \\ddot{β}\\\\0 & 0 & 0\\\\\\sin{\\left(β \\right)} \\dot{β}^{2} - \\cos{\\left(β \\right)} \\ddot{β} & 0 & - \\sin{\\left(β \\right)} \\ddot{β} - \\cos{\\left(β \\right)} \\dot{β}^{2}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\ddot{R}_z = \\left[\\begin{matrix}- \\sin{\\left(α \\right)} \\ddot{α} - \\cos{\\left(α \\right)} \\dot{α}^{2} & \\sin{\\left(α \\right)} \\dot{α}^{2} - \\cos{\\left(α \\right)} \\ddot{α} & 0\\\\- \\sin{\\left(α \\right)} \\dot{α}^{2} + \\cos{\\left(α \\right)} \\ddot{α} & - \\sin{\\left(α \\right)} \\ddot{α} - \\cos{\\left(α \\right)} \\dot{α}^{2} & 0\\\\0 & 0 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Calculate the derivative of dRy and dRz\n",
    "ddRy = (βddt * gy) @ Ry + (βdt * gy) @ dRy\n",
    "ddRz = (αddt * gz) @ Rz + (αdt * gz) @ dRz\n",
    "\n",
    "# View the result\n",
    "ds.display(ds.Math(f\"\\ddot{{R}}_y = {vlatex(sym.Matrix(ddRy))}\"))\n",
    "ds.display(ds.Math(f\"\\ddot{{R}}_z = {vlatex(sym.Matrix(ddRz))}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now recall our derivative of $\\bf{R}(\\Gamma)$ previously defined\n",
    "\n",
    "\\begin{align*}\n",
    "    \\ddot{\\bf{R}}\n",
    "    &=\n",
    "    \\ddot{\\bf{R}_x}\n",
    "    \\bf{R}_y\n",
    "    \\bf{R}_y\n",
    "    +\n",
    "    \\bf{R}_x\n",
    "    \\ddot{\\bf{R}_y}\n",
    "    \\bf{R}_y\n",
    "    +\n",
    "    \\bf{R}_x\n",
    "    \\bf{R}_y\n",
    "    \\ddot{\\bf{R}_y} +\n",
    "    2 \\left(\n",
    "    \\dot{\\bf{R}_x}\n",
    "    \\dot{\\bf{R}_y}\n",
    "    \\bf{R}_y\n",
    "    +\n",
    "    \\dot{\\bf{R}_x}\n",
    "    \\bf{R}_y\n",
    "    \\dot{\\bf{R}_y}\n",
    "    +\n",
    "    \\bf{R}_x\n",
    "    \\dot{\\bf{R}_y}\n",
    "    \\dot{\\bf{R}_y}\n",
    "    \\right)\n",
    "\\end{align*}\n",
    "\n",
    "We now have the necessary components to calculate this in Python"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\ddot{R} = \\left[\\begin{matrix}\\left(- \\sin{\\left(α \\right)} \\ddot{α} - \\cos{\\left(α \\right)} \\dot{α}^{2}\\right) \\cos{\\left(β \\right)} + \\left(- \\sin{\\left(β \\right)} \\ddot{β} - \\cos{\\left(β \\right)} \\dot{β}^{2}\\right) \\cos{\\left(α \\right)} + 2 \\sin{\\left(α \\right)} \\sin{\\left(β \\right)} \\dot{α} \\dot{β} & \\left(\\sin{\\left(α \\right)} \\dot{α}^{2} - \\cos{\\left(α \\right)} \\ddot{α}\\right) \\cos{\\left(β \\right)} - \\left(- \\sin{\\left(β \\right)} \\ddot{β} - \\cos{\\left(β \\right)} \\dot{β}^{2}\\right) \\sin{\\left(α \\right)} + 2 \\sin{\\left(β \\right)} \\cos{\\left(α \\right)} \\dot{α} \\dot{β} & - \\sin{\\left(β \\right)} \\dot{β}^{2} + \\cos{\\left(β \\right)} \\ddot{β}\\\\\\left(- \\sin{\\left(α \\right)} \\dot{α}^{2} + \\cos{\\left(α \\right)} \\ddot{α}\\right) \\cos{\\left(γ \\right)} + \\left(- \\sin{\\left(α \\right)} \\ddot{α} - \\cos{\\left(α \\right)} \\dot{α}^{2}\\right) \\sin{\\left(β \\right)} \\sin{\\left(γ \\right)} - \\left(\\sin{\\left(β \\right)} \\dot{β}^{2} - \\cos{\\left(β \\right)} \\ddot{β}\\right) \\sin{\\left(γ \\right)} \\cos{\\left(α \\right)} - \\left(\\sin{\\left(γ \\right)} \\dot{γ}^{2} - \\cos{\\left(γ \\right)} \\ddot{γ}\\right) \\sin{\\left(β \\right)} \\cos{\\left(α \\right)} + \\left(- \\sin{\\left(γ \\right)} \\ddot{γ} - \\cos{\\left(γ \\right)} \\dot{γ}^{2}\\right) \\sin{\\left(α \\right)} - 2 \\sin{\\left(α \\right)} \\sin{\\left(β \\right)} \\cos{\\left(γ \\right)} \\dot{α} \\dot{γ} - 2 \\sin{\\left(α \\right)} \\sin{\\left(γ \\right)} \\cos{\\left(β \\right)} \\dot{α} \\dot{β} - 2 \\sin{\\left(γ \\right)} \\cos{\\left(α \\right)} \\dot{α} \\dot{γ} + 2 \\cos{\\left(α \\right)} \\cos{\\left(β \\right)} \\cos{\\left(γ \\right)} \\dot{β} \\dot{γ} & \\left(\\sin{\\left(α \\right)} \\dot{α}^{2} - \\cos{\\left(α \\right)} \\ddot{α}\\right) \\sin{\\left(β \\right)} \\sin{\\left(γ \\right)} + \\left(- \\sin{\\left(α \\right)} \\ddot{α} - \\cos{\\left(α \\right)} \\dot{α}^{2}\\right) \\cos{\\left(γ \\right)} + \\left(\\sin{\\left(β \\right)} \\dot{β}^{2} - \\cos{\\left(β \\right)} \\ddot{β}\\right) \\sin{\\left(α \\right)} \\sin{\\left(γ \\right)} + \\left(\\sin{\\left(γ \\right)} \\dot{γ}^{2} - \\cos{\\left(γ \\right)} \\ddot{γ}\\right) \\sin{\\left(α \\right)} \\sin{\\left(β \\right)} + \\left(- \\sin{\\left(γ \\right)} \\ddot{γ} - \\cos{\\left(γ \\right)} \\dot{γ}^{2}\\right) \\cos{\\left(α \\right)} + 2 \\sin{\\left(α \\right)} \\sin{\\left(γ \\right)} \\dot{α} \\dot{γ} - 2 \\sin{\\left(α \\right)} \\cos{\\left(β \\right)} \\cos{\\left(γ \\right)} \\dot{β} \\dot{γ} - 2 \\sin{\\left(β \\right)} \\cos{\\left(α \\right)} \\cos{\\left(γ \\right)} \\dot{α} \\dot{γ} - 2 \\sin{\\left(γ \\right)} \\cos{\\left(α \\right)} \\cos{\\left(β \\right)} \\dot{α} \\dot{β} & - \\left(- \\sin{\\left(β \\right)} \\ddot{β} - \\cos{\\left(β \\right)} \\dot{β}^{2}\\right) \\sin{\\left(γ \\right)} + \\left(\\sin{\\left(γ \\right)} \\dot{γ}^{2} - \\cos{\\left(γ \\right)} \\ddot{γ}\\right) \\cos{\\left(β \\right)} + 2 \\sin{\\left(β \\right)} \\cos{\\left(γ \\right)} \\dot{β} \\dot{γ}\\\\\\left(- \\sin{\\left(α \\right)} \\dot{α}^{2} + \\cos{\\left(α \\right)} \\ddot{α}\\right) \\sin{\\left(γ \\right)} - \\left(- \\sin{\\left(α \\right)} \\ddot{α} - \\cos{\\left(α \\right)} \\dot{α}^{2}\\right) \\sin{\\left(β \\right)} \\cos{\\left(γ \\right)} + \\left(\\sin{\\left(β \\right)} \\dot{β}^{2} - \\cos{\\left(β \\right)} \\ddot{β}\\right) \\cos{\\left(α \\right)} \\cos{\\left(γ \\right)} + \\left(- \\sin{\\left(γ \\right)} \\dot{γ}^{2} + \\cos{\\left(γ \\right)} \\ddot{γ}\\right) \\sin{\\left(α \\right)} - \\left(- \\sin{\\left(γ \\right)} \\ddot{γ} - \\cos{\\left(γ \\right)} \\dot{γ}^{2}\\right) \\sin{\\left(β \\right)} \\cos{\\left(α \\right)} - 2 \\sin{\\left(α \\right)} \\sin{\\left(β \\right)} \\sin{\\left(γ \\right)} \\dot{α} \\dot{γ} + 2 \\sin{\\left(α \\right)} \\cos{\\left(β \\right)} \\cos{\\left(γ \\right)} \\dot{α} \\dot{β} + 2 \\sin{\\left(γ \\right)} \\cos{\\left(α \\right)} \\cos{\\left(β \\right)} \\dot{β} \\dot{γ} + 2 \\cos{\\left(α \\right)} \\cos{\\left(γ \\right)} \\dot{α} \\dot{γ} & - \\left(\\sin{\\left(α \\right)} \\dot{α}^{2} - \\cos{\\left(α \\right)} \\ddot{α}\\right) \\sin{\\left(β \\right)} \\cos{\\left(γ \\right)} + \\left(- \\sin{\\left(α \\right)} \\ddot{α} - \\cos{\\left(α \\right)} \\dot{α}^{2}\\right) \\sin{\\left(γ \\right)} - \\left(\\sin{\\left(β \\right)} \\dot{β}^{2} - \\cos{\\left(β \\right)} \\ddot{β}\\right) \\sin{\\left(α \\right)} \\cos{\\left(γ \\right)} + \\left(- \\sin{\\left(γ \\right)} \\dot{γ}^{2} + \\cos{\\left(γ \\right)} \\ddot{γ}\\right) \\cos{\\left(α \\right)} + \\left(- \\sin{\\left(γ \\right)} \\ddot{γ} - \\cos{\\left(γ \\right)} \\dot{γ}^{2}\\right) \\sin{\\left(α \\right)} \\sin{\\left(β \\right)} - 2 \\sin{\\left(α \\right)} \\sin{\\left(γ \\right)} \\cos{\\left(β \\right)} \\dot{β} \\dot{γ} - 2 \\sin{\\left(α \\right)} \\cos{\\left(γ \\right)} \\dot{α} \\dot{γ} - 2 \\sin{\\left(β \\right)} \\sin{\\left(γ \\right)} \\cos{\\left(α \\right)} \\dot{α} \\dot{γ} + 2 \\cos{\\left(α \\right)} \\cos{\\left(β \\right)} \\cos{\\left(γ \\right)} \\dot{α} \\dot{β} & \\left(- \\sin{\\left(β \\right)} \\ddot{β} - \\cos{\\left(β \\right)} \\dot{β}^{2}\\right) \\cos{\\left(γ \\right)} + \\left(- \\sin{\\left(γ \\right)} \\ddot{γ} - \\cos{\\left(γ \\right)} \\dot{γ}^{2}\\right) \\cos{\\left(β \\right)} + 2 \\sin{\\left(β \\right)} \\sin{\\left(γ \\right)} \\dot{β} \\dot{γ}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "ddR = (\n",
    "    ddRx @ Ry @ Rz\n",
    "    + Rx @ ddRy @ Rz\n",
    "    + Rx @ Ry @ ddRz\n",
    "    + 2 * (dRx @ dRy @ Rz + dRx @ Ry @ dRz + Rx @ dRy @ dRz)\n",
    ")\n",
    "\n",
    "# View the result\n",
    "ds.display(ds.Math(f\"\\ddot{{R}} = {vlatex(sym.Matrix(ddR))}\"))\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now using the identity we derived previously\n",
    "\\begin{align*}\n",
    "    \\ddot{\\bf{R}}\n",
    "    &= \n",
    "    [\\alpha]_\\times \\\n",
    "    \\bf{R}\n",
    "    +\n",
    "    [\\omega]_\\times \\\n",
    "    \\dot{\\bf{R}}\n",
    "\\end{align*}\n",
    "\n",
    "we can rearrange to obtain the angular acceleration $\\alpha$\n",
    "\n",
    "\\begin{align*}\n",
    "    \\ddot{\\bf{R}} -\n",
    "    [\\omega]_\\times \\\n",
    "    \\dot{\\bf{R}}\n",
    "    &= \n",
    "    [\\alpha]_\\times \\\n",
    "    \\bf{R}\n",
    "    \\\\\n",
    "    \\left(\n",
    "    \\ddot{\\bf{R}} -\n",
    "    [\\omega]_\\times \\\n",
    "    \\dot{\\bf{R}}\n",
    "    \\right)\n",
    "    \\dot{\\bf{R}}^\\top\n",
    "    &= \n",
    "    [\\alpha]_\\times\n",
    "    \\\\\n",
    "    \\vee_\\times\n",
    "    \\Bigg(\n",
    "    \\left(\n",
    "    \\ddot{\\bf{R}} -\n",
    "    [\\omega]_\\times \\\n",
    "    \\dot{\\bf{R}}\n",
    "    \\right)\n",
    "    \\dot{\\bf{R}}^\\top\n",
    "    \\Bigg)\n",
    "    &= \n",
    "    \\alpha\n",
    "\\end{align*}\n",
    "\n",
    "Now lets try in Python"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\dot{ω} = \\left[\\begin{matrix}\\sin{\\left(β \\right)} \\ddot{α} + \\cos{\\left(β \\right)} \\dot{α} \\dot{β} + \\ddot{γ}\\\\\\sin{\\left(β \\right)} \\sin{\\left(γ \\right)} \\dot{α} \\dot{β} - \\sin{\\left(γ \\right)} \\cos{\\left(β \\right)} \\ddot{α} - \\sin{\\left(γ \\right)} \\dot{β} \\dot{γ} - \\cos{\\left(β \\right)} \\cos{\\left(γ \\right)} \\dot{α} \\dot{γ} + \\cos{\\left(γ \\right)} \\ddot{β}\\\\- \\sin{\\left(β \\right)} \\cos{\\left(γ \\right)} \\dot{α} \\dot{β} - \\sin{\\left(γ \\right)} \\cos{\\left(β \\right)} \\dot{α} \\dot{γ} + \\sin{\\left(γ \\right)} \\ddot{β} + \\cos{\\left(β \\right)} \\cos{\\left(γ \\right)} \\ddot{α} + \\cos{\\left(γ \\right)} \\dot{β} \\dot{γ}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "ωd = vex((ddR - skew(ω.tolist()) @ dR) @ R.T)\n",
    "\n",
    "# Simplify the result to make it more readable\n",
    "ωd = sym.simplify(ωd)\n",
    "\n",
    "# View the result\n",
    "ds.display(ds.Math(f\"\\dot{{ω}} = {vlatex(sym.Matrix(ωd))}\"))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\dot{ω} = \\left[\\begin{matrix}\\sin{\\left(β \\right)} \\ddot{α} + \\cos{\\left(β \\right)} \\dot{α} \\dot{β} + \\ddot{γ}\\\\\\sin{\\left(β \\right)} \\sin{\\left(γ \\right)} \\dot{α} \\dot{β} - \\sin{\\left(γ \\right)} \\cos{\\left(β \\right)} \\ddot{α} - \\sin{\\left(γ \\right)} \\dot{β} \\dot{γ} - \\cos{\\left(β \\right)} \\cos{\\left(γ \\right)} \\dot{α} \\dot{γ} + \\cos{\\left(γ \\right)} \\ddot{β}\\\\- \\sin{\\left(β \\right)} \\cos{\\left(γ \\right)} \\dot{α} \\dot{β} - \\sin{\\left(γ \\right)} \\cos{\\left(β \\right)} \\dot{α} \\dot{γ} + \\sin{\\left(γ \\right)} \\ddot{β} + \\cos{\\left(β \\right)} \\cos{\\left(γ \\right)} \\ddot{α} + \\cos{\\left(γ \\right)} \\dot{β} \\dot{γ}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "ωd_sym = sym.diff(sym.Matrix(ω))\n",
    "\n",
    "# Simplify the result to make it more readable\n",
    "ωd_sym = sym.simplify(ωd_sym)\n",
    "\n",
    "# View the result\n",
    "ds.display(ds.Math(f\"\\dot{{ω}} = {vlatex(sym.Matrix(ωd_sym))}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We now have a relationship between angular acceleration and roll-pitch-yaw accelerations."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "From Section 3.3, we have the following relationship\n",
    "\n",
    "\\begin{align*}\n",
    "    \\omega =\n",
    "    \\bf{A}(\\Gamma)\n",
    "    \\dot{\\Gamma}\n",
    "\\end{align*}\n",
    "\n",
    "Taking the derivative of this using the product rule we get\n",
    "\n",
    "\\begin{align*}\n",
    "    \\alpha =\n",
    "    \\dot{\\bf{A}}(\\Gamma)\n",
    "    \\dot{\\Gamma}\n",
    "    +\n",
    "    \\bf{A}(\\Gamma)\n",
    "    \\ddot{\\Gamma}\n",
    "\\end{align*}"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "With this in mind, lets try to re-factor the matrix stored in `ωd` into the form above"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\dot{A} \\times \\ddot{Γ} = \\left[\\begin{matrix}\\cos{\\left(β \\right)} \\dot{α} \\dot{β}\\\\\\sin{\\left(β \\right)} \\sin{\\left(γ \\right)} \\dot{α} \\dot{β} - \\sin{\\left(γ \\right)} \\dot{β} \\dot{γ} - \\cos{\\left(β \\right)} \\cos{\\left(γ \\right)} \\dot{α} \\dot{γ}\\\\- \\sin{\\left(β \\right)} \\cos{\\left(γ \\right)} \\dot{α} \\dot{β} - \\sin{\\left(γ \\right)} \\cos{\\left(β \\right)} \\dot{α} \\dot{γ} + \\cos{\\left(γ \\right)} \\dot{β} \\dot{γ}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\dot{A} = \\left[\\begin{matrix}\\cos{\\left(β \\right)} \\dot{β} & 0 & 0\\\\\\sin{\\left(β \\right)} \\sin{\\left(γ \\right)} \\dot{β} - \\cos{\\left(β \\right)} \\cos{\\left(γ \\right)} \\dot{γ} & - \\sin{\\left(γ \\right)} \\dot{γ} & 0\\\\- \\sin{\\left(β \\right)} \\cos{\\left(γ \\right)} \\dot{β} - \\sin{\\left(γ \\right)} \\cos{\\left(β \\right)} \\dot{γ} & \\cos{\\left(γ \\right)} \\dot{γ} & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Since we already know A, lets extract Ad from ωd\n",
    "\n",
    "# Using Algebra, we can find the vector Ad @ Γd = ωd - (Ad @ Γdd)\n",
    "AdΓd = sym.Matrix(ωd) - A @ sym.Matrix(Γdd)\n",
    "ds.display(ds.Math(f\"\\dot{{A}} \\\\times \\ddot{{Γ}} = {vlatex(sym.Matrix(AdΓd))}\"))\n",
    "\n",
    "# Now lets factor out Γd from AdΓd to obtain Ad\n",
    "Ad = sym.Matrix.zeros(3, 3)\n",
    "\n",
    "for i in range(3):\n",
    "    e = ωd[i].expand()\n",
    "    for j in range(3):\n",
    "        Ad[i, j] = e.coeff(Γd[j])\n",
    "        # Remove that term from the e expression\n",
    "        e = sym.simplify(e - Ad[i, j] * Γd[j])\n",
    "\n",
    "# View the result\n",
    "ds.display(ds.Math(f\"\\dot{{A}} = {vlatex(sym.Matrix(Ad))}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Note that $\\dot{\\bf{A}}$ is actually a tensor $\\in \\mathbb{R}^{3 \\times 3 \\times 3}$ multiplied by $\\Gamma$ to form a matrix $\\in \\mathbb{R}^{3 \\times 3}$."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "With this we can write the full equation."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle α = \\left[\\begin{matrix}\\cos{\\left(β \\right)} \\dot{β} & 0 & 0\\\\\\sin{\\left(β \\right)} \\sin{\\left(γ \\right)} \\dot{β} - \\cos{\\left(β \\right)} \\cos{\\left(γ \\right)} \\dot{γ} & - \\sin{\\left(γ \\right)} \\dot{γ} & 0\\\\- \\sin{\\left(β \\right)} \\cos{\\left(γ \\right)} \\dot{β} - \\sin{\\left(γ \\right)} \\cos{\\left(β \\right)} \\dot{γ} & \\cos{\\left(γ \\right)} \\dot{γ} & 0\\end{matrix}\\right] \\left[\\begin{matrix}\\dot{α}\\\\\\dot{β}\\\\\\dot{γ}\\end{matrix}\\right] + \\left[\\begin{matrix}\\sin{\\left(β \\right)} & 0 & 1\\\\- \\sin{\\left(γ \\right)} \\cos{\\left(β \\right)} & \\cos{\\left(γ \\right)} & 0\\\\\\cos{\\left(β \\right)} \\cos{\\left(γ \\right)} & \\sin{\\left(γ \\right)} & 0\\end{matrix}\\right]\\left[\\begin{matrix}\\ddot{α}\\\\\\ddot{β}\\\\\\ddot{γ}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# View in expanded form in Python\n",
    "ds.display(\n",
    "    ds.Math(\n",
    "        f\"α = {vlatex(sym.Array(Ad))} {vlatex(sym.Matrix(Γd))} + {vlatex(sym.Matrix(A))}{vlatex(sym.Matrix(Γdd))}\"\n",
    "    )\n",
    ")"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Next, we create the $\\dot{\\bf{A}}^{-1}$ matrix"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\dot{A}^{-1} = \\left[\\begin{matrix}0 & - \\frac{\\frac{\\sin{\\left(β \\right)} \\sin{\\left(γ \\right)} \\dot{β}}{\\cos{\\left(β \\right)}} + \\cos{\\left(γ \\right)} \\dot{γ}}{\\cos{\\left(β \\right)}} & \\frac{\\frac{\\sin{\\left(β \\right)} \\cos{\\left(γ \\right)} \\dot{β}}{\\cos{\\left(β \\right)}} - \\sin{\\left(γ \\right)} \\dot{γ}}{\\cos{\\left(β \\right)}}\\\\0 & - \\sin{\\left(γ \\right)} \\dot{γ} & \\cos{\\left(γ \\right)} \\dot{γ}\\\\0 & \\frac{\\sin{\\left(γ \\right)} \\dot{β}}{\\cos^{2}{\\left(β \\right)}} + \\cos{\\left(γ \\right)} \\tan{\\left(β \\right)} \\dot{γ} & \\sin{\\left(γ \\right)} \\tan{\\left(β \\right)} \\dot{γ} - \\frac{\\cos{\\left(γ \\right)} \\dot{β}}{\\cos^{2}{\\left(β \\right)}}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# unfortunately synpy cannot invert out Ad matrix. We will cheat and use symbolic\n",
    "# differentiation on the Ai matrix to obtain the derivative of the inverse of A\n",
    "Adi = sym.trigsimp(sym.diff(sym.Matrix(Ai)))\n",
    "\n",
    "# View the result\n",
    "ds.display(ds.Math(f\"\\dot{{A}}^{{-1}} = {vlatex(sym.Matrix(Adi))}\"))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Lets turn $\\dot{\\bf{A}}$ and $\\dot{\\bf{A}}^{-1}$ into Python code"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 32,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The Ad matrix: np.array([[βd*math.cos(β), 0, 0], [βd*math.sin(β)*math.sin(γ) - γd*math.cos(β)*math.cos(γ), -γd*math.sin(γ), 0], [-βd*math.sin(β)*math.cos(γ) - γd*math.sin(γ)*math.cos(β), γd*math.cos(γ), 0]])\n",
      "The Adi matrix: np.array([[0, -(βd*math.sin(β)*math.sin(γ)/math.cos(β) + γd*math.cos(γ))/math.cos(β), (βd*math.sin(β)*math.cos(γ)/math.cos(β) - γd*math.sin(γ))/math.cos(β)], [0, -γd*math.sin(γ), γd*math.cos(γ)], [0, βd*math.sin(γ)/math.cos(β)**2 + γd*math.cos(γ)*math.tan(β), -βd*math.cos(γ)/math.cos(β)**2 + γd*math.sin(γ)*math.tan(β)]])\n"
     ]
    }
   ],
   "source": [
    "py_Ad = sym.pycode(\n",
    "    Ad.subs(a for a in zip(Γd, Γd_py)).subs(a for a in zip(Γ, Γ_py))\n",
    ").replace(\"ImmutableDenseMatrix\", \"np.array\")\n",
    "py_Adi = sym.pycode(\n",
    "    Adi.subs(a for a in zip(Γd, Γd_py)).subs(a for a in zip(Γ, Γ_py))\n",
    ").replace(\"ImmutableDenseMatrix\", \"np.array\")\n",
    "\n",
    "# View the results\n",
    "print(f\"The Ad matrix: {py_Ad}\")\n",
    "print(f\"The Adi matrix: {py_Adi}\")\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can then copy and paste these into Python methods"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 33,
   "metadata": {},
   "outputs": [],
   "source": [
    "def JAd(Γ, Γd):\n",
    "    \"\"\"\n",
    "    Returns the derivative of the Jacobian which relates\n",
    "    angular velocity to xyz roll-pitch-yaw rates.\n",
    "\n",
    "    Γ is a 3 vector representing the current RPY angles\n",
    "    Γd is a 3 vector representing the current RPY angle rates\n",
    "    \"\"\"\n",
    "    α, β, γ = Γ\n",
    "    αd, βd, γd = Γd\n",
    "\n",
    "    return np.array(\n",
    "        [\n",
    "            [βd * math.cos(β), 0, 0],\n",
    "            [\n",
    "                βd * math.sin(β) * math.sin(γ) - γd * math.cos(β) * math.cos(γ),\n",
    "                -γd * math.sin(γ),\n",
    "                0,\n",
    "            ],\n",
    "            [\n",
    "                -βd * math.sin(β) * math.cos(γ) - γd * math.sin(γ) * math.cos(β),\n",
    "                γd * math.cos(γ),\n",
    "                0,\n",
    "            ],\n",
    "        ]\n",
    "    )\n",
    "\n",
    "\n",
    "def JAdi(Γ, Γd):\n",
    "    \"\"\"\n",
    "    Returns the derivative of the Jacobian which relates xyz\n",
    "    roll-pitch-yaw rates to angular velocity.\n",
    "\n",
    "    Γ is a 3 vector representing the current RPY angles\n",
    "    Γd is a 3 vector representing the current RPY angle rates\n",
    "    \"\"\"\n",
    "    α, β, γ = Γ\n",
    "    αd, βd, γd = Γd\n",
    "\n",
    "    return np.array(\n",
    "        [\n",
    "            [\n",
    "                0,\n",
    "                -(βd * math.sin(β) * math.sin(γ) / math.cos(β) + γd * math.cos(γ))\n",
    "                / math.cos(β),\n",
    "                (βd * math.sin(β) * math.cos(γ) / math.cos(β) - γd * math.sin(γ))\n",
    "                / math.cos(β),\n",
    "            ],\n",
    "            [0, -γd * math.sin(γ), γd * math.cos(γ)],\n",
    "            [\n",
    "                0,\n",
    "                βd * math.sin(γ) / math.cos(β) ** 2 + γd * math.cos(γ) * math.tan(β),\n",
    "                -βd * math.cos(γ) / math.cos(β) ** 2 + γd * math.sin(γ) * math.tan(β),\n",
    "            ],\n",
    "        ]\n",
    "    )\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='Ja'></a>\n",
    "\n",
    "### 3.5 The Analytic Jacobian\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The kinematic derivatives we have presented in previous Notebooks so far have been in geometric form with translational velocity $\\vec{v}$ and angular velocity $\\vec{\\omega}$ vectors. Some applications require the manipulator Jacobian and further derivatives to to be expressed with different orientation rate representations such as the rate of change of Euler angles, roll-pitch-yaw angles or exponential coordinates -- these are called analytical forms.\n",
    "\n",
    "In the first four Sections of this Notebook, we have outlined how to translate angular velocity and acceleration into roll-pitch-yaw rates and accelerations. In this section we are going to exploit this relationship to obtain the analytic manipulator Jacobian.\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The analytical Jacobian represented with roll-pitch-yaw angle rates is\n",
    "\n",
    "\\begin{align*}\n",
    "    \\bf{J}_a(\\bf{q})\n",
    "    &=\n",
    "    \\tilde{\\bf{J}}(\\bf{\\Gamma}) \\ \\bf{J}(\\bf{q})\n",
    "    \\\\\n",
    "    &=\n",
    "    \\begin{pmatrix}\n",
    "        \\bf{1}_{3 \\times 3} & \\bf{0}_{3 \\times 3} \\\\\n",
    "        \\bf{0}_{3 \\times 3} & \\bf{A}^{-1}(\\bf{\\Gamma}) \\\\\n",
    "    \\end{pmatrix}\n",
    "    \\bf{J}(\\bf{q}).\n",
    "\\end{align*}\n",
    "\n",
    "Lets turn this into a python method"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 34,
   "metadata": {},
   "outputs": [],
   "source": [
    "def jacob0_rpy(robot, q):\n",
    "    \"\"\"\n",
    "    Returns the analytic base-frame manipulator Jacobian where\n",
    "    the rotation units are expressed as xyz roll-pitch-yaw rates\n",
    "\n",
    "    robot is an Robot object representing a robot\n",
    "    q is the current joint coordinates of the robot\n",
    "    \"\"\"\n",
    "\n",
    "    # Calculate the forward kinematics of the robot\n",
    "    T = robot.fkine(q)\n",
    "\n",
    "    # Calculate the rpy of the orientation within T\n",
    "    Γ = T.rpy(order='xyz')\n",
    "\n",
    "    # Calculate the Jacobian which relates angular velocity to\n",
    "    # rpy rates\n",
    "    Ai = JAi(Γ)\n",
    "\n",
    "    # Construct the augmented Jacobian\n",
    "    Jaug = np.zeros((6, 6))\n",
    "\n",
    "    # Top left corner is an identity\n",
    "    Jaug[:3, :3] = np.eye(3)\n",
    "\n",
    "    # Bottom right corner is Ai\n",
    "    Jaug[3:, 3:] = Ai\n",
    "\n",
    "    # Calculate the base-frame manipulator Jacobian\n",
    "    J0 = robot.jacob0(q)\n",
    "\n",
    "    # Calculate the analytic Jacobian\n",
    "    Ja = Jaug @ J0\n",
    "\n",
    "    return Ja"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Lets test it out"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 35,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Ja using our method:\n",
      "[[ 0.    0.08  0.    0.25  0.    0.2   0.  ]\n",
      " [ 0.48  0.    0.49  0.    0.15 -0.    0.  ]\n",
      " [-0.   -0.48  0.    0.5  -0.    0.11  0.  ]\n",
      " [-1.01 -0.   -0.96  0.    0.32  0.    1.  ]\n",
      " [ 0.   -1.    0.    1.    0.    1.   -0.  ]\n",
      " [ 0.1  -0.   -0.2  -0.    0.91 -0.    0.  ]]\n",
      "\n",
      "Ja using the toolbox:\n",
      "[[ 0.    0.08  0.    0.25  0.    0.2   0.  ]\n",
      " [ 0.48  0.    0.49  0.    0.15 -0.    0.  ]\n",
      " [-0.   -0.48  0.    0.5  -0.    0.11  0.  ]\n",
      " [-1.01 -0.   -0.96  0.    0.32  0.    1.  ]\n",
      " [ 0.   -1.    0.    1.    0.    1.   -0.  ]\n",
      " [ 0.1  -0.   -0.2  -0.    0.91 -0.    0.  ]]\n"
     ]
    }
   ],
   "source": [
    "# Make a robot\n",
    "robot = rtb.models.Panda()\n",
    "\n",
    "# Set the joint angles\n",
    "q = robot.qr.copy()\n",
    "\n",
    "# Calculate the analytic Jacobian\n",
    "Ja = jacob0_rpy(robot, q)\n",
    "\n",
    "# View the result\n",
    "print(\"Ja using our method:\")\n",
    "print(np.round(Ja, 2))\n",
    "\n",
    "# Compare to using the roboticstoolbox method\n",
    "print(\"\\nJa using the toolbox:\")\n",
    "Ja_rtb = robot.jacob0_analytical(q, representation=\"rpy/xyz\")\n",
    "print(np.round(Ja_rtb, 2))\n",
    "\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Using the `robot.jacob0_analytical` method within the `roboticstoolbox` we can calculate the analytic Jacobian in many forms:\n",
    "\n",
    "        ==================   ==================================\n",
    "         representation           Rotational representation\n",
    "        ==================   ==================================\n",
    "         'rpy/xyz'            RPY angular rates in XYZ order\n",
    "         'rpy/zyx'            RPY angular rates in XYZ order\n",
    "         'eul'                Euler angular rates in ZYZ order\n",
    "         'exp'                exponential coordinate rates\n",
    "        ==================   =================================="
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 36,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Ja (rpy/xyz):\n",
      "[[ 0.    0.08  0.    0.25  0.    0.2   0.  ]\n",
      " [ 0.48  0.    0.49  0.    0.15 -0.    0.  ]\n",
      " [-0.   -0.48  0.    0.5  -0.    0.11  0.  ]\n",
      " [-1.01 -0.   -0.96  0.    0.32  0.    1.  ]\n",
      " [ 0.   -1.    0.    1.    0.    1.   -0.  ]\n",
      " [ 0.1  -0.   -0.2  -0.    0.91 -0.    0.  ]]\n",
      "\n",
      "Ja (rpy/zyx):\n",
      "[[ 0.    0.08  0.    0.25  0.    0.2   0.  ]\n",
      " [ 0.48  0.    0.49  0.    0.15 -0.    0.  ]\n",
      " [-0.   -0.48  0.    0.5  -0.    0.11  0.  ]\n",
      " [-0.   -0.   -0.3  -0.    0.95 -0.    0.1 ]\n",
      " [-0.    1.    0.   -1.   -0.   -1.   -0.  ]\n",
      " [ 1.    0.    0.98  0.   -0.42  0.   -1.01]]\n",
      "\n",
      "Ja (eul):\n",
      "[[ 0.    0.08  0.    0.25  0.    0.2   0.  ]\n",
      " [ 0.48  0.    0.49  0.    0.15 -0.    0.  ]\n",
      " [-0.   -0.48  0.    0.5  -0.    0.11  0.  ]\n",
      " [ 1.   -0.   -1.99  0.    9.11  0.    0.  ]\n",
      " [-0.    1.   -0.   -1.    0.   -1.    0.  ]\n",
      " [-0.   -0.   -2.96  0.    9.48  0.    1.  ]]\n",
      "\n",
      "Ja (exp):\n",
      "[[ 0.    0.08  0.    0.25  0.    0.2   0.  ]\n",
      " [ 0.48  0.    0.49  0.    0.15 -0.    0.  ]\n",
      " [-0.   -0.48  0.    0.5  -0.    0.11  0.  ]\n",
      " [ 0.05  0.08 -0.25 -0.08  0.93 -0.08  0.05]\n",
      " [ 1.57  0.    1.52  0.   -0.58  0.   -1.57]\n",
      " [ 0.   -1.57 -0.01  1.57  0.05  1.57  0.  ]]\n"
     ]
    }
   ],
   "source": [
    "print(\"Ja (rpy/xyz):\")\n",
    "Ja_rtb1 = robot.jacob0_analytical(q, representation=\"rpy/xyz\")\n",
    "print(np.round(Ja_rtb1, 2))\n",
    "\n",
    "print(\"\\nJa (rpy/zyx):\")\n",
    "Ja_rtb2 = robot.jacob0_analytical(q, representation=\"rpy/zyx\")\n",
    "print(np.round(Ja_rtb2, 2))\n",
    "\n",
    "print(\"\\nJa (eul):\")\n",
    "Ja_rtb3 = robot.jacob0_analytical(q, representation=\"eul\")\n",
    "print(np.round(Ja_rtb3, 2))\n",
    "\n",
    "print(\"\\nJa (exp):\")\n",
    "Ja_rtb4 = robot.jacob0_analytical(q, representation=\"exp\")\n",
    "print(np.round(Ja_rtb4, 2))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='Jad'></a>\n",
    "\n",
    "### 3.6 Derivative of the Analytic Jacobian\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The derivative of $\\bf{J}_a(\\bf{q})$ is typically used in applications which have a task-space acceleration term. We can obtain $\\dot{\\bf{J}}_a(\\bf{q})$ from the definition of $\\bf{J}_a(\\bf{q})$ using the product rule\n",
    "\n",
    "\\begin{align*} \n",
    "    \\dot{\\bf{J}}_a(\\bf{q})\n",
    "    &=\n",
    "    \\dfrac{\\text{d} \\tilde{\\bf{J}}(\\bf{\\Gamma})}\n",
    "          {\\text{d} t} \\\n",
    "    \\bf{J}(\\bf{q}) +\n",
    "    \\tilde{\\bf{J}}(\\bf{\\Gamma}) \\ \n",
    "    \\dfrac{\\text{d} \\bf{J}(\\bf{q})}\n",
    "          {\\text{d} t} \\\\\n",
    "    &=\n",
    "    \\dfrac{\\text{d} \\tilde{\\bf{J}}(\\bf{\\Gamma})}\n",
    "          {\\text{d} t} \\\n",
    "    \\bf{J}(\\bf{q}) +\n",
    "    \\tilde{\\bf{J}}(\\bf{\\Gamma}) \\ \n",
    "    \\big(\n",
    "        \\bf{H}(\\bf{q}) \\dot{\\bf{q}}\n",
    "    \\big)\n",
    "\\end{align*}"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Taking the derivative of $\\tilde{\\bf{J}}(\\bf{\\Gamma})$ gives\n",
    "\n",
    "\\begin{align*}\n",
    "    \\dfrac{\\text{d} \\tilde{\\bf{J}}(\\bf{\\Gamma})}\n",
    "          {\\text{d} t}\n",
    "    &=\n",
    "    \\begin{pmatrix}\n",
    "        \\bf{0}_{3 \\times 3} & \\bf{0}_{3 \\times 3} \\\\\n",
    "        \\bf{0}_{3 \\times 3} & \\dot{\\bf{A}}^{-1}(\\bf{\\Gamma, \\ \\dot{\\Gamma}}) \\\\\n",
    "    \\end{pmatrix}\n",
    "\\end{align*}"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Lets turn this into a python method"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 37,
   "metadata": {},
   "outputs": [],
   "source": [
    "def jacob0dot_rpy(robot, q, qd):\n",
    "    \"\"\"\n",
    "    Returns the analytic base-frame manipulator Jacobian where\n",
    "    the rotation units are expressed as xyz roll-pitch-yaw rates\n",
    "\n",
    "    robot is an Robot object representing a robot\n",
    "    q is the current joint coordinates of the robot\n",
    "    \"\"\"\n",
    "\n",
    "    # Calculate the forward kinematics of the robot\n",
    "    T = robot.fkine(q)\n",
    "\n",
    "    # Calculate the base-frame manipulator Jacobian\n",
    "    J0 = robot.jacob0(q)\n",
    "\n",
    "    # Calculate the base-frame manipulator Hessian\n",
    "    H0 = robot.hessian0(q)\n",
    "\n",
    "    # Calculate the end-effector velocity and extract angular velocity\n",
    "    ev = J0 @ qd\n",
    "    eω = ev[3:]\n",
    "\n",
    "    # Calculate the rpy of the orientation within T\n",
    "    Γ = T.rpy(order='xyz')\n",
    "\n",
    "    # Calculate the rpy rate\n",
    "    Γd = JAi(Γ) @ eω\n",
    "\n",
    "    # Calculate the Jacobian which relates angular velocity to\n",
    "    # rpy rates\n",
    "    Ai = JAi(Γ)\n",
    "\n",
    "    # Calculate the derivative of theJacobian which relates angular\n",
    "    # velocity to rpy rates\n",
    "    Adi = JAdi(Γ, Γd)\n",
    "\n",
    "    # Construct the augmented Jacobian\n",
    "    Jaug = np.zeros((6, 6))\n",
    "\n",
    "    # Top left corner is an identity\n",
    "    Jaug[:3, :3] = np.eye(3)\n",
    "\n",
    "    # Bottom right corner is Ai\n",
    "    Jaug[3:, 3:] = Ai\n",
    "\n",
    "    # Construct the augmented Jacobian derivative\n",
    "    Jaugd = np.zeros((6, 6))\n",
    "\n",
    "    # Bottom right corner is Ai\n",
    "    Jaugd[3:, 3:] = Adi\n",
    "\n",
    "    # Calculate the analytic Jacobian derivative\n",
    "    Jad = Jaugd @ J0 + Jaug @ np.tensordot(H0, qd, (0, 0))\n",
    "\n",
    "    return Jad"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Lets test it out"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 38,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Jad using our method:\n",
      "[[-0.23  0.01 -0.22 -0.02 -0.05  0.02  0.  ]\n",
      " [-0.1   0.01 -0.11  0.14  0.05  0.05  0.  ]\n",
      " [-0.    0.1  -0.05 -0.04  0.02 -0.04  0.  ]\n",
      " [ 0.02  0.13  0.08 -0.22 -0.1  -0.03  0.  ]\n",
      " [-0.13 -0.   -0.1   0.   -0.29  0.    0.  ]\n",
      " [-0.2  -0.11 -0.39  0.41  0.11  0.33 -0.  ]]\n",
      "\n",
      "Jad using the toolbox:\n",
      "[[-0.23  0.01 -0.22 -0.02 -0.05  0.02  0.  ]\n",
      " [-0.1   0.01 -0.11  0.14  0.05  0.05  0.  ]\n",
      " [ 0.    0.1  -0.05 -0.04  0.02 -0.04  0.  ]\n",
      " [ 0.02  0.13  0.08 -0.22 -0.1  -0.03 -0.  ]\n",
      " [-0.13  0.   -0.1  -0.   -0.29 -0.    0.  ]\n",
      " [-0.2  -0.11 -0.39  0.41  0.11  0.33  0.  ]]\n"
     ]
    }
   ],
   "source": [
    "# Make a robot\n",
    "robot = rtb.models.Panda()\n",
    "\n",
    "# Set the joint angles\n",
    "q = robot.qr.copy()\n",
    "\n",
    "# Set the joint rates\n",
    "qd = np.array([0.1, -0.2, 0.3, -0.1, 0.2, -0.3, 0.4])\n",
    "\n",
    "# Calculate the analytic Jacobian derivative\n",
    "Jad = jacob0dot_rpy(robot, q, qd)\n",
    "\n",
    "# View the result\n",
    "print(\"Jad using our method:\")\n",
    "print(np.round(Jad, 2))\n",
    "\n",
    "# Compare to using the roboticstoolbox method\n",
    "print(\"\\nJad using the toolbox:\")\n",
    "Jad_rtb = robot.jacob0_dot(q, qd, representation=\"rpy/xyz\")\n",
    "print(np.round(Jad_rtb, 2))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Using the `robot.jacob0_dot` method within the `roboticstoolbox` we can calculate the derivative of the analytic Jacobian in many forms using the `representation` kwarg:\n",
    "\n",
    "        ==================   ==================================\n",
    "         representation           Rotational representation\n",
    "        ==================   ==================================\n",
    "         'rpy/xyz'            RPY angular rates in XYZ order\n",
    "         'rpy/zyx'            RPY angular rates in XYZ order\n",
    "         'eul'                Euler angular rates in ZYZ order\n",
    "         'exp'                exponential coordinate rates\n",
    "        ==================   =================================="
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 39,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Jad (rpy/xyz):\n",
      "[[-0.23  0.01 -0.22 -0.02 -0.05  0.02  0.  ]\n",
      " [-0.1   0.01 -0.11  0.14  0.05  0.05  0.  ]\n",
      " [ 0.    0.1  -0.05 -0.04  0.02 -0.04  0.  ]\n",
      " [ 0.02  0.13  0.08 -0.22 -0.1  -0.03 -0.  ]\n",
      " [-0.13  0.   -0.1  -0.   -0.29 -0.    0.  ]\n",
      " [-0.2  -0.11 -0.39  0.41  0.11  0.33  0.  ]]\n",
      "\n",
      "Jad (rpy/zyx):\n",
      "[[-0.23  0.01 -0.22 -0.02 -0.05  0.02  0.  ]\n",
      " [-0.1   0.01 -0.11  0.14  0.05  0.05  0.  ]\n",
      " [ 0.    0.1  -0.05 -0.04  0.02 -0.04  0.  ]\n",
      " [ 0.   -0.19 -0.19  0.48  0.01  0.41 -0.2 ]\n",
      " [ 0.   -0.   -0.06  0.    0.42  0.    0.14]\n",
      " [-0.    0.02 -0.1   0.04  0.28 -0.14  0.02]]\n",
      "\n",
      "Jad (eul):\n",
      "[[-2.300e-01  1.000e-02 -2.200e-01 -2.000e-02 -5.000e-02  2.000e-02\n",
      "   0.000e+00]\n",
      " [-1.000e-01  1.000e-02 -1.100e-01  1.400e-01  5.000e-02  5.000e-02\n",
      "   0.000e+00]\n",
      " [ 0.000e+00  1.000e-01 -5.000e-02 -4.000e-02  2.000e-02 -4.000e-02\n",
      "   0.000e+00]\n",
      " [-0.000e+00  1.221e+01 -7.890e+00 -9.260e+00  1.941e+01 -1.009e+01\n",
      "   0.000e+00]\n",
      " [ 0.000e+00 -0.000e+00  3.600e-01  0.000e+00 -9.200e-01  0.000e+00\n",
      "   0.000e+00]\n",
      " [ 0.000e+00  1.227e+01 -7.810e+00 -9.400e+00  1.922e+01 -1.004e+01\n",
      "   0.000e+00]]\n",
      "\n",
      "Jad (exp):\n",
      "[[-0.23  0.01 -0.22 -0.02 -0.05  0.02  0.  ]\n",
      " [-0.1   0.01 -0.11  0.14  0.05  0.05  0.  ]\n",
      " [ 0.    0.1  -0.05 -0.04  0.02 -0.04  0.  ]\n",
      " [-0.03 -0.3  -0.23  0.59  0.08  0.52 -0.16]\n",
      " [ 0.01  0.01 -0.11  0.11  0.25 -0.18 -0.01]\n",
      " [-0.01 -0.02  0.08  0.03 -0.68  0.03 -0.22]]\n"
     ]
    }
   ],
   "source": [
    "print(\"Jad (rpy/xyz):\")\n",
    "Jad_rtb1 = robot.jacob0_dot(q, qd, representation=\"rpy/xyz\")\n",
    "print(np.round(Jad_rtb1, 2))\n",
    "\n",
    "print(\"\\nJad (rpy/zyx):\")\n",
    "Jad_rtb2 = robot.jacob0_dot(q, qd, representation=\"rpy/zyx\")\n",
    "print(np.round(Jad_rtb2, 2))\n",
    "\n",
    "print(\"\\nJad (eul):\")\n",
    "Jad_rtb3 = robot.jacob0_dot(q, qd, representation=\"eul\")\n",
    "print(np.round(Jad_rtb3, 2))\n",
    "\n",
    "print(\"\\nJad (exp):\")\n",
    "Jad_rtb4 = robot.jacob0_dot(q, qd, representation=\"exp\")\n",
    "print(np.round(Jad_rtb4, 2))"
   ]
  }
 ],
 "metadata": {
  "interpreter": {
   "hash": "528815e074ebcdb9b34bcb695d4aa9d425bdb2cc6656b4ca45050b51a4125937"
  },
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
